﻿//-----------------------------------------------------------------------
//  This file is part of University of Georgia Code Samples.
//
//  Copyright (C) UGA.  All rights reserved.
//
//  $File: Mapper
//  Description:
//  Mapper class represents the map object. It has the following attributes
//          - grid resolution:
//          - grid row:  number of pixel rows
//          - grid column: number of pixel columns
//          - mapdata: a 2D array of Color representing the map
//          - mapImage: a bitmap representing the map
//          
//-----------------------------------------------------------------------
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Drawing.Imaging;
using System.Xml;

//using laser = Microsoft.Robotics.Services.Sensors.SickLRF.Proxy;

namespace NestedParticleFilter
{
    
    public class Map
    {
        #region Constant Parameters 

        // map dimensions
        
        public const double MapResolution = 0.06; // in meters
        public const double XMinCorner = -2.8;
        public const double XMaxCorner = 14.0;
        public const double XMinHallCorner = 8.94;
        public const double XMaxHallCorner = 10.4;
        public const double ZMinCorner = -9.20;
        public const double ZMaxCorner = 7.65;
        public const double ZMinHallCorner = 6.17;
        public const double ZMaxHallCorner = 7.65;
        public static readonly Color TargetColor = Color.IndianRed;
        private static readonly Color WallColor = Color.White;
        public static readonly Color BackgroundColor = Color.LightGray;
        private static readonly Color RobotIColor = Color.Black;

        /// <summary>
        /// Gird Width in meters
        /// </summary>
        public const double GridWidth = 16.8; // for particle generation only
        /// <summary>
        /// Grid height in meters
        /// </summary>
        public const double GridHeight = 16.85; // for particle generation only

        #endregion

       
        private static readonly List<Landmark> _walls = new List<Landmark>(); // real world position of the walls
        private static readonly List<Landmark> _dynamicWalls = new List<Landmark>(); //real world positon of the dynamic landmarks
        
        ////////////////////////////////////////////////////////////////////////////////////////// 
        //                                          Class members                                          //
        ////////////////////////////////////////////////////////////////////////////////////////
        private readonly double _mapWidth;
        private readonly double _mapHeight;
        private readonly int _gridRow; // grid dimensions
        private readonly int _gridCol; // grid dimensions
        private readonly Color[,] _mapData; // map of the building with colored landmarks
        private bool _readWalls;
        private bool _drawWalls;
        private Color _particleColor;
        private String _configPath; // path to the configuration file.

        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="width">width of the map</param>
        /// <param name="height">height of the map</param>
        /// <param name="particleColor">color of the particle</param>
        /// <param name="configPath">path to the configuration file of the simulation engine</param>
        public  Map(double width, double height, Color particleColor, String configPath)
        {

            _mapWidth = width;
            _mapHeight = height;
            //number of rows and columns
            _gridCol = (int)Math.Ceiling(_mapWidth / MapResolution);
            _particleColor = particleColor;
            _gridRow = (int)Math.Ceiling(_mapHeight / MapResolution);
            _configPath = configPath;
            // Ensure that the row has a multiple of 4 pixels,
            // i.e. it will be longword aligned when the bitmap
            // image is created
            if ((_gridCol % 4) != 0)
                _gridCol += 4 - (_gridCol % 4);
            _mapData = new Color[_gridRow, _gridCol];
            // colloring the map with the background color. otherwise the backgroud willbe black
            ClearMap(_mapData, _gridRow, _gridCol);
            InitializeMap();// draw walls sand dynamic landmarks
            
        }


        /// <summary>
        /// MapToImage - Convert the mapData to an image
        /// </summary>
        /// <remarks>
        /// Creates a bitmap from the map array
        /// NOTE: The bitmap will be 24-bit RGB which is the most common format.
        /// The bitmap will be greyscale, as is usual for an occpancy grid,
        /// with Black = Occupied, White = Free.
        /// However, coloured information can be written into it by the
        /// caller, e.g. a path for the robot, after the bitmap is returned.
        /// The _mapData is a 2D array and inorder to make a bitmap we have to convert it to 
        /// a 1D array first and then pass it to MakeBitmap function where it creates a bitmap
        ///  outof a 1D array.
        /// </remarks>
        /// <returns>Bitmap containing the map (24-bit RGB)</returns>
        public Bitmap GetBitmap()
        {
            var bmp = new Bitmap(_gridCol, _gridRow, PixelFormat.Format24bppRgb);
            // Lock the bitmap's bits.  
            Rectangle rect = new Rectangle(0, 0, bmp.Width, bmp.Height);
            BitmapData bmpData =
                bmp.LockBits(rect, ImageLockMode.ReadWrite, bmp.PixelFormat);
            
            // Get the address of the first line.
            IntPtr ptr = bmpData.Scan0;

            // Declare an array to hold the bytes of the bitmap.
            int bytes  = Math.Abs(bmpData.Stride) * bmp.Height;
            byte[] rgbValues = new byte[bytes];

            // Copy the RGB values into the array.
            System.Runtime.InteropServices.Marshal.Copy(ptr, rgbValues, 0, bytes);

            // Set every third value to 255. A 24bpp bitmap will look red. 
            int i = 0, j = 0;
            for (int counter = 0; counter < rgbValues.Length; )
            {
                rgbValues[counter++] = _mapData[i, j].B;
                rgbValues[counter++] = _mapData[i, j].G;
                rgbValues[counter++] = _mapData[i, j].R;
                j++;
                if (j >= _gridCol)
                {
                    i++;
                    j = 0;
                }
            }

            // Copy the RGB values back to the bitmap
            System.Runtime.InteropServices.Marshal.Copy(rgbValues, 0, ptr, bytes);

            // Unlock the bits.
            bmp.UnlockBits(bmpData);
            return bmp;
        }

        /// <summary>
        /// Clear() - Erase the map
        /// </summary>
        public static  void ClearMap(Color [,] map, int row, int col)
        {
            int i, j;
            for (j = 0; j < col; j++)
                for (i = 0; i < row; i++)
                    map[i, j] = BackgroundColor;

            return;
        }

        /// <summary>
        /// update the map object
        /// </summary>
        private void InitializeMap()
        {
            if (!_readWalls)
                ReadWalls();
            if (!_drawWalls)
            {
                DrawWalls();
                DrawDynamicLandmarks();
            }
        }

        /// <summary>
        /// update the map with the new particles
        /// </summary>
        /// <param name="particles">robot's particles</param>
        public void UpdateMap (List<Particle> particles)
        {
            if (!_readWalls|| !_drawWalls)  
                InitializeMap();
           ClearPrevParticles();
           AddParticles(particles);
        }


        /// <summary>
        /// update the map with the new particles and robot's location
        /// </summary>
        /// <param name="pose">pose of the robot</param>
        /// <param name="particles">particles representing the possible locations of the robot </param>
        public void UpdateMap(RobotPose pose, List<Particle> particles)
        {
            if (!_readWalls || !_drawWalls)
                InitializeMap();
            ClearRobot();
            ClearPrevParticles();
            AddParticles(particles);
            try
            {
                AddRobotToMap(pose);
            }
            catch(Exception e)
            {
                Debug.WriteLine("######Could not add robot to map " + e.ToString());
            }
        }

        /// <summary>
        /// update the map with the new particles and robots' locations in case of multiple robots
        /// </summary>
        /// <param name="iPose">pose of robot I</param>
        /// <param name="jPose">pose of robot J</param>
        /// <param name="particles">particles representing the possible locations of the robot </param>
        public void UpdateMap(RobotPose iPose, RobotPose jPose, List<Particle> particles)
        {
            if (!_readWalls || !_drawWalls)
                InitializeMap();
            ClearRobot();
            ClearPrevParticles();
            AddParticles(particles);
            try
            {
                AddRobotToMap(iPose);
                AddRobotToMap(jPose);
            }
            catch (Exception e)
            {
                Debug.WriteLine("######Could not add robot to map " + e.ToString());
            }
        }

        /// <summary>
        /// update the map depicting the nested particles. This method only prints out the particles of robot j given
        /// the list of all the particles of robot i. 
        /// </summary>
        /// <param name="particles">particles of robot i </param>
        /// <exception cref="ArgumentNullException"><paramref name="particles" /> is <c>null</c>.</exception>
        public void UpdateMapWithNestedParticles(List<Particle> particles)
        {
            if (particles == null)
                throw new ArgumentNullException("particles", "List may not be null");
            if (!_readWalls || !_drawWalls)
                InitializeMap();
            ClearRobot();
            ClearPrevParticles(_particleColor);
            for (int i = 0; i < particles.Count;i++ )
                AddParticles(particles[i].NestedParticles);
        }

        /// <summary>
        /// update the map depicting the nested particles and robots' locations. This method only prints out the particles of robot j given
        /// the list of all the particles of robot i. 
        /// </summary>
        /// <param name="iPose">pose of robot I</param>
        /// <param name="jPose">pose of robot J</param>
        /// <param name="particles">particles of robot i </param>
        public void UpdateMapWithNestedParticles(RobotPose iPose, RobotPose jPose, List<Particle> particles)
        {
            if (!_readWalls || !_drawWalls)
                InitializeMap();
            ClearRobot();
            ClearPrevParticles(_particleColor);
            for (int i = 0; i < particles.Count; i++ )
                AddParticles(particles[i].NestedParticles);
            try
            {
                AddRobotToMap(iPose);
                AddRobotToMap(jPose);
            }
            catch (Exception e)
            {
                Debug.WriteLine("Could not add robot to map " + e.ToString());
            }
        }
        /******************************************************************************************/
       
        #region Coordinate Conversion

        /// <summary>
        /// converts the real-world X coord to map X coord
        /// </summary>
        /// <param name="x">x</param>
        /// <param name="column">number of columns</param>
        /// <returns>x in map</returns>
        public static int WorldToMapXCoordinate(double x, int  column)
        {
            return (int) Math.Floor(x/MapResolution) + column/2;
        }

        /// <summary>
        /// converts the real-world Z coord to map Z coord
        /// </summary>
        /// <param name="z">Z</param>
        /// <param name="row">Number of rows </param>
        /// <returns>z in map</returns>
        public static int WorldToMapZCoordinate(double z, int row)
        {
            return (int) Math.Floor(z / MapResolution) + row / 2  ;
        }

        /// <summary>
        /// This function converts the map coordinate into simulator coordinate
        /// </summary>
        /// <param name="mapX">map coordinate X</param>
        /// <returns>real X coordinate of the simulator environment</returns>
        private double MapToWorldXCoordinate(int mapX)
        {
            return MapResolution * (mapX - (double)_gridCol / 2);
        }

        /// <summary>
        /// This function converts the map Z coordinate into simulator coordinate
        /// </summary>
        /// <param name="mapZ">map coordinate Z</param>
        /// <returns>real Z coordinate of the simulator environment</returns>
        private double MapToWorldZCoordinate(int mapZ)
        {
            return MapResolution * (mapZ - (double)_gridRow / 2);
        }

        /// <summary>
        /// converts the real world theta to map theta. That includes (0,45,90,135, 180,-45,-90,-135)
        /// </summary>
        /// <param name="theta">real world theta</param>
        /// <returns>map theta </returns>
        public static int WorldToMapTheta(double theta)
        {
            int angle = 0;
            if (theta >= -22.5 && theta < 22.5)
                angle = 0;
            else if (theta >= 22.5 && theta < 67.5)
                angle = 45;
            else if (theta >= 67.5 && theta < 112.5)
                angle = 90;
            else if (theta >= 112.5 && theta < 157.5)
                angle = 135;
            else if (theta >= 157.5 || theta <= -135 || theta == -0)
                angle = 180;
            else if (theta >= -67.5 && theta < -22.5)
                angle = -45;
            else if (theta >= -112.5 && theta < -67.5)
                angle = -90;
            else if (theta >= -157.5 && theta < -112.5)
                angle = -135;
            return angle;
        }

        /// <summary>
        /// validates that the given input coordinates are in the map
        /// </summary>
        /// <param name="x">x position in the map</param>
        /// <param name="z">z position in the map</param>
        /// <param name="col">number of columns in the grid map</param>
        /// <param name="row">number of rows in the grid map</param>
        /// <returns>true if the inputs are in the map, and false otherwise</returns>
        public static bool ValidatePointInMap(double x, double z, double col, double row)
        {
            return (x >= 0) && (x < col) &&
                   (z >= 0) && (z < row);
        }
        #endregion

        /******************************************************************************************/
        #region Draw Target
        public void DrawTarget (int row, int col)
        {
            Debug.Assert(row < _gridRow && col < _gridCol,
                         "Row and col should be less than grid row and gird col. Row:  " + row + " Col: " + col + " Grid Row: "+ _gridRow  + " Grid Col: "+ _gridCol);
            _mapData[row, col] = TargetColor;
        }

        #endregion 

        /******************************************************************************************/
        
        #region Drawing Particles
        /// <summary>
        /// Draw Particles on the map
        /// </summary>
        /// <param name="particles">list of particles</param>
        private void AddParticles(List<Particle> particles)
        {
            if (particles != null)
                for (int i = 0; i < particles.Count; i++)
                {
                   DrawParticles(particles[i].Pose.X, particles[i].Pose.Z);
                }
        }
        /// <summary>
        /// Draw particles on the map with the specified color
        /// </summary>
        /// <param name="particles">list of particles</param>
        /// <param name="color">color of the particles</param>
        private void AddParticles(List<Particle> particles, Color color)
        {
            if (particles != null)
                for (int i = 0; i < particles.Count; i++)
                {
                    DrawParticles(particles[i].Pose.X, particles[i].Pose.Z, color);
                }
        }
        
        /// <summary>
        /// Add a particle to the map
        /// </summary>
        /// <param name="x">Real x coordinate of the particle</param>
        /// <param name="z">Real z coordinate of the particle</param>
        private void DrawParticles(double x, double z)
        {
            int mx = WorldToMapXCoordinate(x,_gridCol);
            int mz = WorldToMapZCoordinate(z, _gridRow);
           
                _mapData[mz, mx] = _particleColor;
        }

        /// <summary>
        /// Add a particle to the map
        /// </summary>
        /// <param name="x">Real x coordinate of the particle</param>
        /// <param name="z">Real z coordinate of the particle</param>
        /// <param name="color">color of the particle</param>
        private void DrawParticles(double x, double z, Color color)
        {
            int mx = WorldToMapXCoordinate(x,_gridCol);
            int mz = WorldToMapZCoordinate(z, _gridRow);
          

                _mapData[mz, mx] = color;
        }

        /// <summary>
        /// Clear previous particles drawn on the map
        /// </summary>
        private void ClearPrevParticles()
        {
            for (int i = 0; i < _gridRow; i++)
                for (int j = 0; j < _gridCol; j++)
                {
                    if (_mapData[i, j] == _particleColor)
                        _mapData[i, j] = BackgroundColor;
                }
            //draw the walls again. This is needed due to clearing the map from particles
            DrawWalls();
        }

        /// <summary>
        /// Clear previous particles on the map with the specified color
        /// </summary>
        /// <param name="color">color of the particles</param>
        private void ClearPrevParticles(Color color)
        {
            for (int i = 0; i < _gridRow; i++)
                for (int j = 0; j < _gridCol; j++)
                {
                    if (_mapData[i, j] == color)
                        _mapData[i, j] = BackgroundColor;
                }
            //draw the walls again. This is needed due to clearing the map from particles
            DrawWalls();
        }
        #endregion
       
        /******************************************************************************************/
        
        #region Draw Robot

        /// <summary>
        /// Add a single robot to the map, and redraw walls. 
        /// </summary>
        /// <param name="pose">Real world pose of the robot</param>
        private void AddRobotToMap(RobotPose pose)
        {
            if (pose == null)
                return;
            
            // clear the traps of the robot.
            //ClearRobot();
            int mx = WorldToMapXCoordinate(pose.X, _gridCol);
            int mz = WorldToMapZCoordinate(pose.Z, _gridRow);
            int angle = WorldToMapTheta(pose.Theta);
            // draw robot 
            DrawRobot(mx, mz, angle, _mapData, RobotIColor, _gridRow,_gridCol);
            if (!_drawWalls)
                DrawWalls();
        }

        /// <summary>
        /// Plot the robots on the map
        /// </summary>
        /// <param name="poses">Real world poses of the robots</param>
        private void AddRobotsToMap(RobotPose[] poses)
        {
            // clear other traps of the robot.
            ClearRobot();
            int count = poses.Length; //number of robots
            for (int i = 0; i < count; i++) // for each robot
            {
                int mx = WorldToMapXCoordinate(poses[i].X,_gridCol);
                int mz = WorldToMapZCoordinate(poses[i].Z, _gridRow);
                int angle = WorldToMapTheta(poses[i].Theta);
                // draw robot 
                DrawRobot(mx, mz, angle, _mapData, RobotIColor, _gridRow,_gridCol);
            }
            if (!_drawWalls)
                DrawWalls();
        }

        /// <summary>
        /// Draw the robot on the map in a cirlce shape
        /// </summary>
        /// <param name="x">map based robot's x position</param>
        /// <param name="z">map based robot's y position</param>
        /// <param name="angle">map based robot's angle in Degrees</param>
        /// <param name="map">2D array of color map</param>
        /// <param name="color">Color of the robot</param>
        /// <param name="row">number of  rows in the map</param>
        /// <param name="col">number of columns in the map</param>
        /// <exception cref="ArgumentNullException"><paramref name="map" /> is <c>null</c>.</exception>
        /// <exception cref="Exception">invalid coordinate</exception>
        public static void DrawRobot(int x, int z, int angle, Color [,] map, Color color, int row, int col)
        {
            if (map == null)
                throw new ArgumentNullException("map");
             if (!ValidatePointInMap(x, z, col, row))
                throw new Exception("invalid coordinate");
             
            // draw a circle around the center of the robot.
            // circle has the radius of 3 pixels
            for (int i = z - 1; i <= z + 1; i++)
            {
                map[i, x - 3] = color;
                map[i, x + 3] = color;
            }
            for (int i = x - 1; i <= x + 1; i++)
            {
                map[z - 3, i] = color;
                map[z + 3, i] = color;
            }
            map[z - 2, x - 2] = color;
            map[z - 2, x + 2] = color;
            map[z + 2, x - 2] = color;
            map[z + 2, x + 2] = color;

            // draw the orientation of the robot based on the angle
            if (angle == 180)
            {
                map[z + 1, x] = color;
                map[z + 2, x] = color;
            }
            else if (angle == 0)
            {
                map[z - 1, x] = color;
                map[z - 2, x] = color;
            }
            else if (angle == 90)
            {
                map[z, x - 1] = color;
                map[z, x - 2] = color;
            }
            else if (angle == -90)
            {
                map[z, x + 1] = color;
                map[z, x + 2] = color;
            }
            else if (angle == 135)
                map[z + 1, x - 1] = color;

            else if (angle == -135)
                map[z + 1, x + 1] = color;

            else if (angle == 45)
                map[z - 1, x - 1] = color;

            else if (angle == -45)
                map[z - 1, x + 1] = color;
        }

        
        /// <summary>
        /// Clears the robot from the map
        /// </summary>
        private void ClearRobot()
        {
            for (int i = 0; i < _gridRow; i++)
                for (int j = 0; j < _gridCol; j++)
                {
                    if (_mapData[i, j] == RobotIColor)
                        _mapData[i, j] = BackgroundColor;
                }
        }
        #endregion

        /**********************************************************************************************/

        #region Draw Walls and Dynamic Landmarks
        
        /// <summary>
        /// Draw the walls on the map. The wall list must have been populated already. 
        /// This means the configuration file must have been parsed prior to the call to this 
        /// function
        /// </summary>
        private void DrawWalls()
        {
            int i = 0;
            if (_walls!= null && _walls.Count!= 0)
                while (i < _walls.Count)
                {
                    double x1 = _walls[i].P1.X;
                    double x2 = _walls[i].P2.X;
                    double x3 = _walls[i].P3.X;
                    double x4 = _walls[i].P4.X;
                    double z1 = _walls[i].P1.Z;
                    double z2 = _walls[i].P2.Z;
                    double z3 = _walls[i].P3.Z;
                    double z4 = _walls[i].P4.Z;

                    x1 = WorldToMapXCoordinate(x1,_gridCol);
                    x2 = WorldToMapXCoordinate(x2, _gridCol);
                    x3 = WorldToMapXCoordinate(x3, _gridCol);
                    x4 = WorldToMapXCoordinate(x4, _gridCol);
                    z1 = WorldToMapZCoordinate(z1, _gridRow);
                    z2 = WorldToMapZCoordinate(z2, _gridRow);
                    z3 = WorldToMapZCoordinate(z3, _gridRow);
                    z4 = WorldToMapZCoordinate(z4, _gridRow);
                    Color wallColor = _walls[i].LandmarkColor;
                    double minX = Math.Min(x1, Math.Min(x2, Math.Min(x3, x4)));
                    double maxX = Math.Max(x1, Math.Max(x2, Math.Max(x3, x4)));
                    double minZ = Math.Min(z1, Math.Min(z2, Math.Min(z3, z4)));
                    double maxZ = Math.Max(z1, Math.Max(z2, Math.Max(z3, z4)));


                    for (var row = (int) minZ; row <= maxZ; row++)
                        for (var col = (int) minX; col <= maxX; col++)
                        {
                            if (_mapData != null)
                                if (_mapData[row, col] != wallColor)
                                    _mapData[row, col] = wallColor;
                        }
                    i++;
                }
            _drawWalls = true;
        }
      

        /// <summary>
        /// Draw a dynamic landmark on the map. This function is to update the map every time the dynamic 
        /// object has moved.
        /// </summary>
        /// <param name="dynamicWall">dynamic landmark</param>
        public void DrawDynamicLandmark(Landmark dynamicWall)
        {
            DrawLine(dynamicWall.P1, dynamicWall.P2, dynamicWall.LandmarkColor);
            DrawLine(dynamicWall.P1, dynamicWall.P3, dynamicWall.LandmarkColor);
            DrawLine(dynamicWall.P3, dynamicWall.P4, dynamicWall.LandmarkColor);
            DrawLine(dynamicWall.P2, dynamicWall.P4, dynamicWall.LandmarkColor);
            FloodFill(dynamicWall.Center, dynamicWall.LandmarkColor);
        }

        /// <summary>
        /// Draw all dynamic landmarks in _dynamicWalls
        /// </summary>
        public void DrawDynamicLandmarks()
        {
            if (_dynamicWalls != null)
                foreach (Landmark landmark in _dynamicWalls)
                {
                    DrawDynamicLandmark(landmark);
                }
        }

        /// <summary>
        /// Clear a specific dynamic landmark from the map
        /// </summary>
        /// <param name="dynamicWall">dynamic landmark to be erased</param>
        public void ClearDynamicLandmark(Landmark dynamicWall)
        {
            double x1 = dynamicWall.P1.X;
            double x2 = dynamicWall.P2.X;
            double x3 = dynamicWall.P3.X;
            double x4 = dynamicWall.P4.X;
            double z1 = dynamicWall.P1.Z;
            double z2 = dynamicWall.P2.Z;
            double z3 = dynamicWall.P3.Z;
            double z4 = dynamicWall.P4.Z;

            Color wallColor = dynamicWall.LandmarkColor;

            x1 = WorldToMapXCoordinate(x1,_gridCol);
            x2 = WorldToMapXCoordinate(x2, _gridCol);
            x3 = WorldToMapXCoordinate(x3, _gridCol);
            x4 = WorldToMapXCoordinate(x4, _gridCol);
            z1 = WorldToMapZCoordinate(z1, _gridRow);
            z2 = WorldToMapZCoordinate(z2, _gridRow);
            z3 = WorldToMapZCoordinate(z3,_gridRow);
            z4 = WorldToMapZCoordinate(z4,_gridRow);

            double minX = Math.Min(x1, Math.Min(x2, Math.Min(x3, x4)));
            double maxX = Math.Max(x1, Math.Max(x2, Math.Max(x3, x4)));
            double minZ = Math.Min(z1, Math.Min(z2, Math.Min(z3, z4)));
            double maxZ = Math.Max(z1, Math.Max(z2, Math.Max(z3, z4)));


            for (int i = (int) minZ - 7; i < maxZ + 7; i++)
                for (int j = (int) minX - 7; j < maxX + 7; j++)
                {
                    if (i >= 0 && j >= 0 && i < _gridRow && j < _gridCol)
                    {
                        //if (_mapData[i, j] == wallColor)
                            _mapData[i, j] = BackgroundColor;
                    }
                }
        }
        #endregion 

        /************************************************************************************/
       
        #region Read XML Configuration
        /// <summary>
        /// Read the simulation config file, parses the file to find the center of the walls as 
        /// well as the dimensions of the walls. and it saves the corner xs and zs of each wall
        /// into a list and return the list
        /// </summary>
        private  void ReadWalls()
        {
            try
            {
                // create an xml document and read the file
                var doc = new XmlDocument();
                doc.Load(_configPath);

                //get all the nodes with the tag "SingleShapeEntity"
                XmlNodeList xmlnode = doc.GetElementsByTagName("SingleShapeEntity");

                // for all the shapes find the poses and dimensions
                for (int i = 0; i < xmlnode.Count; i++)
                {
                    //Sim:State is the first child of the node and includes the poses
                    XmlNode state = xmlnode[i].FirstChild;

                    //BoxShape is the last child of the node and includes the dimensions
                    XmlNode boxshape = xmlnode[i].LastChild;


                    if (boxshape.Name.Equals("SphereShape", StringComparison.CurrentCultureIgnoreCase))
                        continue;

                    // name of each wall
                    String name = state.FirstChild.ChildNodes[0].Value;

                    // get the center of the walls
                    XmlNodeList poses = state.ChildNodes[2].FirstChild.ChildNodes;

                    double xc = double.Parse(poses[0].InnerText);
                    double zc = double.Parse(poses[2].InnerText);
                    var center = new Point(xc, zc);

                    XmlNodeList orientation = state.ChildNodes[2].ChildNodes[1].ChildNodes;
                    double thetaAroundY = (Math.Acos(double.Parse(orientation[3].InnerText))*2*180/Math.PI);

                    //get the mass of the wall
                    XmlNode massNode = state.ChildNodes[5].FirstChild;
                    double mass = double.Parse(massNode.InnerText);

                    // get the dimension of the wall
                    XmlNodeList dimension = boxshape.FirstChild.ChildNodes[2].ChildNodes;

                    double length = double.Parse(dimension[0].InnerText);
                    double width = double.Parse(dimension[2].InnerText);


                    if (thetaAroundY > 0)
                    {
                        double temp = length;
                        length = width;
                        width = temp;
                    }
                    // get the real color of the landmark
                    XmlNodeList boxState = boxshape.FirstChild.ChildNodes;
                    /********** Two ways to get the color, for yello we use the texture *********/
                    /********** for green , red and blue we use diffuse color values ***********/
                    Color c;
                    // get the texture
                    String color = boxState[7].InnerText;
                    if (color.Contains("green"))
                    {
                        c = Color.LimeGreen;
                    }
                    else if (color.Contains("pink"))
                        c = Color.Magenta;
                    else if (color.Contains("blue"))
                        c = Color.Blue;
                    else
                        c = WallColor;
                    //else
                    //{
                    //    double r = double.Parse(boxState[8].ChildNodes[0].InnerText);
                    //    double g = double.Parse(boxState[8].ChildNodes[1].InnerText);
                    //    double b = double.Parse(boxState[8].ChildNodes[2].InnerText);
                    //    if (r == 1 && g == 0 && b == 1)
                    //        c = Color.Purple;
                    //    else if (r == 0 && b == 1 && g == 0)
                    //        c = Color.Blue;
                    //    else if (r == 0 && b == 0 && g > 0.5)
                    //        c = Color.DarkGreen;
                    //    else
                    //        c = Color.White;
                    //}
                    var wallinstance = new Landmark(name, center, length, width, c);

                    if (mass == 0)
                    {
                        wallinstance.IsDynamic = false;
                        _walls.Add(wallinstance);
                    }
                    else
                    {
                        wallinstance.IsDynamic = true;
                        _dynamicWalls.Add(wallinstance);
                    }
                }
                _readWalls = true;
                Debug.WriteLine("There are " + _walls.Count + " wall entities in the environment.");
                Debug.WriteLine("There are " + _dynamicWalls.Count + " dynamic entities in the environment");
            }
            catch (Exception ex)
            {
                Console.WriteLine(ex.ToString());
            }
        }
        #endregion 
        
        /*******************************************************************************************************/
       
        #region Draw Helper Methods
        /************************************************************************************/

        /// <summary>
        /// Given the two reference poses, this algorithm will draw the line between these two poses
        /// </summary>
        /// <param name="p1">First point</param>
        /// <param name="p2">second point</param>
        /// <param name="color">color of the line</param>
        /*************************************************************************************/
        private void DrawLine(Point p1, Point p2, Color color)
        {
            // change the coordinates to the map coordinates
            double x1 = WorldToMapXCoordinate(p1.X, _gridCol);
            double x2 = WorldToMapXCoordinate(p2.X, _gridCol);
            double z1 = WorldToMapZCoordinate(p1.Z, _gridRow);
            double z2 = WorldToMapZCoordinate(p2.Z, _gridRow);

            bool steep = Math.Abs(z2 - z1) < Math.Abs(x2 - x1);

            if (steep)
            {
                double temp = x1;
                x1 = z1;
                z1 = temp;

                temp = x2;
                x2 = z2;
                z2 = temp;
            }

            if (z1 > z2)
            {
                double temp = x1;
                x1 = x2;
                x2 = temp;

                temp = z1;
                z1 = z2;
                z2 = temp;
            }

            var deltax = (int) Math.Abs(x2 - x1);
            var deltaz = (int) (z2 - z1);

            int error = deltaz/2;

            int xstep;
            var x = (int) x1;
            if (x1 < x2)
                xstep = 1;
            else
                xstep = -1;
            for (var z = (int) z1; z <= z2; z++)
            {
                if (steep)
                    _mapData[x, z] = color;
                else
                    _mapData[z, x] = color;
                error = error - deltax;
                if (error < 0)
                {
                    x = x + xstep;
                    error = error + deltaz;
                }
            }
        }

        /**************************************************************************************/

        /// <summary>
        ///  FloodFill, will fill in the shape given the center of the shape
        /// </summary>
        /// <param name="center">Center of the shape</param>
        /// <param name="color">color</param>
        /**************************************************************************************/
        private void FloodFill(Point center, Color color)
        {
            int x = WorldToMapXCoordinate(center.X, _gridCol);
            int z = WorldToMapZCoordinate(center.Z, _gridRow);
            var X = new Queue<int>();
            var Z = new Queue<int>();
            if (_mapData[z, x] == color)
                return;
            X.Enqueue(x);
            Z.Enqueue(z);

            while (X.Count != 0 && Z.Count != 0)
            {
                x = X.Dequeue();
                z = Z.Dequeue();
                if (ValidatePointInMap(x, z,_gridCol,_gridRow) && ValidatePointInMap(x + 1, z + 1, _gridCol,_gridRow) 
                    && ValidatePointInMap(x - 1, z - 1, _gridCol,_gridRow))
                {
                    if (_mapData[z, x] == BackgroundColor)
                        _mapData[z, x] = color;


                    if (_mapData[z + 1, x] == BackgroundColor)
                    {
                        _mapData[z + 1, x] = color;
                        X.Enqueue(x);
                        Z.Enqueue(z + 1);
                    }
                    if (_mapData[z - 1, x] == BackgroundColor)
                    {
                        _mapData[z - 1, x] = color;
                        X.Enqueue(x);
                        Z.Enqueue(z - 1);
                    }
                    if (_mapData[z, x + 1] == BackgroundColor)
                    {
                        _mapData[z, x + 1] = color;
                        X.Enqueue(x + 1);
                        Z.Enqueue(z);
                    }
                    if (_mapData[z, x - 1] == BackgroundColor)
                    {
                        _mapData[z, x - 1] = color;
                        X.Enqueue(x - 1);
                        Z.Enqueue(z);
                    }
                }
            } //end of while
            return;
        }
        #endregion

        /******************************************************************************************************/



    }

//end of class mapper
}

